#include "arm_control/arm_action_type2.hpp"
#include <chrono>
#include <thread>
#include <algorithm>

//A区左右抓取动作

namespace arm_control
{

ArmActionType2::ArmActionType2(rclcpp::Logger logger, rclcpp::Node::SharedPtr node)
  : ArmActionBase(logger), node_(node)
{
  // 创建订阅者
  left_sub_ = node_->create_subscription<fruit_detector::msg::DetectionArray>(
    "/detect_left", 10,
    std::bind(&ArmActionType2::left_callback, this, std::placeholders::_1));
  
  right_sub_ = node_->create_subscription<fruit_detector::msg::DetectionArray>(
    "/detect_right", 10,
    std::bind(&ArmActionType2::right_callback, this, std::placeholders::_1));
  
  // 创建爪子发布器
  zhuazi_left_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_left", 10);
  zhuazi_right_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_right", 10);
    
  // 初始化 MoveIt 接口
  move_group_left_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_left");
  move_group_right_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_right");
  
  RCLCPP_INFO(logger_, "动作类型2已创建");
  RCLCPP_INFO(logger_, "订阅话题: /detect_left 和 /detect_right");
  RCLCPP_INFO(logger_, "创建爪子发布器: /zhuazi_left 和 /zhuazi_right");
  RCLCPP_INFO(logger_, "MoveIt接口已初始化: arm_left 和 arm_right");
  RCLCPP_INFO(logger_, "arm_left规划参考坐标系: %s", move_group_left_->getPlanningFrame().c_str());
  RCLCPP_INFO(logger_, "arm_right规划参考坐标系: %s", move_group_right_->getPlanningFrame().c_str());
}

void ArmActionType2::left_callback(const fruit_detector::msg::DetectionArray::SharedPtr msg)
{
  left_data_.clear();
  for (const auto& fruit : msg->fruits) {
    FruitData data;
    data.type = fruit.type;
    data.offset_x = fruit.offset_x;
    data.offset_y = fruit.offset_y;
    data.isgood = fruit.isgood;
    left_data_.push_back(data);
  }
  RCLCPP_DEBUG(logger_, "收到左侧检测数据，共 %zu 个水果", left_data_.size());
}

void ArmActionType2::right_callback(const fruit_detector::msg::DetectionArray::SharedPtr msg)
{
  right_data_.clear();
  for (const auto& fruit : msg->fruits) {
    FruitData data;
    data.type = fruit.type;
    data.offset_x = fruit.offset_x;
    data.offset_y = fruit.offset_y;
    data.isgood = fruit.isgood;
    right_data_.push_back(data);
  }
  RCLCPP_DEBUG(logger_, "收到右侧检测数据，共 %zu 个水果", right_data_.size());
}

FruitData ArmActionType2::select_best_fruit(
  const std::vector<int32_t>& type_array,
  const std::vector<FruitData>& data,
  bool& found)
{
  found = false;
  FruitData best_fruit;
  float min_offset = std::numeric_limits<float>::max();
  
  // 遍历所有数据
  for (const auto& fruit : data) {
    // 检查是否在类型数组中
    auto it = std::find(type_array.begin(), type_array.end(), fruit.type);
    if (it != type_array.end()) {
      // 计算offset
      float offset = fruit.get_offset();
      // 如果offset更小，则更新最佳水果
      if (offset < min_offset) {
        min_offset = offset;
        best_fruit = fruit;
        found = true;
      }
    }
  }
  
  return best_fruit;
}

bool ArmActionType2::move_down(
  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group,
  double distance,
  const std::string& arm_name,
  double offset_x,
  double offset_y)
{
  RCLCPP_INFO(logger_, "[%s] 开始移动: 向下=%.3f米, offset_x=%.2f, offset_y=%.2f", 
              arm_name.c_str(), distance, offset_x, offset_y);
  
  // ========== xyz同时规划 ==========
  RCLCPP_INFO(logger_, "[%s] === xyz同时规划 ===", arm_name.c_str());
  
  // 更新当前状态
  move_group->setStartStateToCurrentState();
  RCLCPP_INFO(logger_, "[%s] 已更新当前机械臂状态", arm_name.c_str());
  
  // 获取当前末端位姿
  geometry_msgs::msg::PoseStamped current_pose = move_group->getCurrentPose();
  RCLCPP_INFO(logger_, "[%s] 当前末端位置: x=%.3f, y=%.3f, z=%.3f", 
              arm_name.c_str(),
              current_pose.pose.position.x,
              current_pose.pose.position.y,
              current_pose.pose.position.z);
  
  // 创建路径点（同时调整x、y、z）
  std::vector<geometry_msgs::msg::Pose> waypoints;
  geometry_msgs::msg::Pose target_pose = current_pose.pose;
  
  // 根据左臂或右臂使用不同的调整公式
  if (arm_name.find("左") != std::string::npos || arm_name.find("left") != std::string::npos) {
    // 左臂：x为offset_x * 0.0008，y为-offset_y * 0.0008
    target_pose.position.x += offset_x * 0.0007;
    target_pose.position.y -= offset_y * 0.001;
  } else {
    // 右臂：x为-offset_x * 0.0008，y为offset_y * 0.0008
    target_pose.position.x -= offset_x * 0.0007;
    target_pose.position.y += offset_y * 0.001;
  }
  // z轴向下移动
  target_pose.position.z -= distance;
  
  waypoints.push_back(target_pose);
  
  RCLCPP_INFO(logger_, "[%s] 目标位置: x=%.3f, y=%.3f, z=%.3f (xyz同时移动)", 
              arm_name.c_str(),
              target_pose.position.x,
              target_pose.position.y,
              target_pose.position.z);
  
  // 计算笛卡尔路径（xyz同时规划）
  moveit_msgs::msg::RobotTrajectory trajectory;
  const double jump_threshold = 0.0;  // 禁用跳跃检测
  const double eef_step = 0.01;       // 1cm 步长
  
  double fraction = move_group->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
  
  RCLCPP_INFO(logger_, "[%s] 笛卡尔路径规划完成度: %.2f%%", arm_name.c_str(), fraction * 100.0);
  
  if (fraction < 0.80) {  // 至少完成80%的路径
    RCLCPP_ERROR(logger_, "[%s] 笛卡尔路径规划失败，完成度不足", arm_name.c_str());
    return false;
  }
  
  // 执行轨迹（xyz同时移动）
  RCLCPP_INFO(logger_, "[%s] 开始执行xyz同时运动...", arm_name.c_str());
  auto execute_result = move_group->execute(trajectory);
  
  if (execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    RCLCPP_ERROR(logger_, "[%s] 运动执行失败！", arm_name.c_str());
    return false;
  }
  
  RCLCPP_INFO(logger_, "[%s] xyz同时运动执行成功！", arm_name.c_str());
  return true;
}

void ArmActionType2::execute(
  const std::shared_ptr<GoalHandleARM> goal_handle,
  std::shared_ptr<ARM::Feedback> feedback,
  std::shared_ptr<ARM::Result> result)
{
  RCLCPP_INFO(logger_, "执行动作类型2 - 机械臂抓取物体（左右两侧）");
  
  const auto goal = goal_handle->get_goal();
  
  // 获取左右侧类型数组
  const auto& left_types = goal->left_type;
  const auto& right_types = goal->right_type;
  
  RCLCPP_INFO(logger_, "左侧类型数组大小: %zu, 右侧类型数组大小: %zu", 
              left_types.size(), right_types.size());
  
  // 等待一段时间确保已收到检测数据
  feedback->progress_percentage = 5.0;
  feedback->current_status = "等待检测数据...";
  goal_handle->publish_feedback(feedback);
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  
  // 检查是否取消
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型2已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型2已取消");
    return;
  }
  
  std::string result_message = "动作类型2执行成功";
  int successful_grabs = 0;
  
  // ========== 第一步：抓取左侧水果 ==========
  if (!left_types.empty()) {
    feedback->progress_percentage = 10.0;
    feedback->current_status = "分析左侧检测数据...";
    goal_handle->publish_feedback(feedback);
    
    bool left_found = false;
    FruitData left_fruit = select_best_fruit(left_types, left_data_, left_found);
    
    if (left_found) {
      RCLCPP_INFO(logger_, "左侧找到匹配水果: 类型=%d, offset=%.2f", 
                  left_fruit.type, left_fruit.get_offset());
      
      // 执行左侧抓取动作
      feedback->progress_percentage = 20.0;
      feedback->current_status = "准备抓取左侧水果...";
      goal_handle->publish_feedback(feedback);
      
      RCLCPP_INFO(logger_, "目标水果(左侧): 类型=%d, offset_x=%.2f, offset_y=%.2f, isgood=%d", 
                  left_fruit.type, left_fruit.offset_x, 
                  left_fruit.offset_y, left_fruit.isgood);
      
      // 使用 MoveIt 控制左侧机械臂向下移动 0.1m
      feedback->progress_percentage = 25.0;
      feedback->current_status = "更新左侧机械臂状态...";
      goal_handle->publish_feedback(feedback);
      
      // 先更新机械臂状态
      RCLCPP_INFO(logger_, "更新左侧机械臂起始状态为当前状态");
      move_group_left_->setStartStateToCurrentState();
      
      feedback->progress_percentage = 30.0;
      feedback->current_status = "正在执行左侧机械臂向下运动...";
      goal_handle->publish_feedback(feedback);
      
      if (goal_handle->is_canceling()) {
        result->success = false;
        result->message = "动作类型2已取消(左侧抓取中)";
        goal_handle->canceled(result);
        RCLCPP_INFO(logger_, "动作类型2已取消(左侧抓取中)");
        return;
      }
      
      bool left_move_success = move_down(move_group_left_, 0.37, "左臂", 
                                          left_fruit.offset_x, left_fruit.offset_y);
      
      if (left_move_success) {
        RCLCPP_INFO(logger_, "左侧水果抓取完成");
        
        // 发布左爪子数据0（关闭）
        auto left_msg = std_msgs::msg::Int32();
        left_msg.data = 0;
        zhuazi_left_publisher_->publish(left_msg);
        RCLCPP_INFO(logger_, "发布到 /zhuazi_left: 0 (关闭)");
        
        // 等待1.5秒让爪子动作完成
        RCLCPP_INFO(logger_, "等待1.5秒让爪子动作完成...");
        std::this_thread::sleep_for(std::chrono::milliseconds(1500));
        
        result_message += " - 左侧抓取成功";
        result_message += " - 左侧(类型=" + std::to_string(left_fruit.type) + 
                         ", offset=" + std::to_string(left_fruit.get_offset()) + ")";
        successful_grabs++;
        
        feedback->progress_percentage = 40.0;
        feedback->current_status = "左侧抓取完成";
        goal_handle->publish_feedback(feedback);
      } else {
        RCLCPP_WARN(logger_, "左侧机械臂运动失败");
        result_message += " - 左侧运动失败";
      }
      
    } else {
      RCLCPP_WARN(logger_, "左侧未找到匹配的水果");
      result_message += " - 左侧未找到匹配水果";
    }
  } else {
    RCLCPP_INFO(logger_, "左侧类型数组为空，跳过左侧抓取");
  }
  
  // 左臂执行完成后，等待1秒
  feedback->progress_percentage = 45.0;
  feedback->current_status = "等待1秒让机械臂稳定...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "左臂执行完成，等待1秒...");
  std::this_thread::sleep_for(std::chrono::seconds(1));
  
  // 更新右臂起始状态为当前状态（包括左臂执行后改变的共享关节状态）
  RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态...");
  move_group_right_->setStartStateToCurrentState();
  
  // 检查是否取消
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型2已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型2已取消");
    return;
  }
  
  // ========== 第二步：抓取右侧水果 ==========
  if (!right_types.empty()) {
    feedback->progress_percentage = 50.0;
    feedback->current_status = "分析右侧检测数据...";
    goal_handle->publish_feedback(feedback);
    
    bool right_found = false;
    FruitData right_fruit = select_best_fruit(right_types, right_data_, right_found);
    
    if (right_found) {
      RCLCPP_INFO(logger_, "右侧找到匹配水果: 类型=%d, offset=%.2f", 
                  right_fruit.type, right_fruit.get_offset());
      
      // 执行右侧抓取动作
      feedback->progress_percentage = 60.0;
      feedback->current_status = "准备抓取右侧水果...";
      goal_handle->publish_feedback(feedback);
      
      RCLCPP_INFO(logger_, "目标水果(右侧): 类型=%d, offset_x=%.2f, offset_y=%.2f, isgood=%d", 
                  right_fruit.type, right_fruit.offset_x, 
                  right_fruit.offset_y, right_fruit.isgood);
      
      // 使用 MoveIt 控制右侧机械臂向下移动 0.1m
      feedback->progress_percentage = 65.0;
      feedback->current_status = "更新右侧机械臂状态...";
      goal_handle->publish_feedback(feedback);
      
      // 先更新机械臂状态（包括左臂执行后的共享关节状态）
      RCLCPP_INFO(logger_, "更新右侧机械臂起始状态为当前状态");
      move_group_right_->setStartStateToCurrentState();
      
      feedback->progress_percentage = 70.0;
      feedback->current_status = "正在执行右侧机械臂向下运动...";
      goal_handle->publish_feedback(feedback);
      
      if (goal_handle->is_canceling()) {
        result->success = false;
        result->message = "动作类型2已取消(右侧抓取中)";
        goal_handle->canceled(result);
        RCLCPP_INFO(logger_, "动作类型2已取消(右侧抓取中)");
        return;
      }
      
      bool right_move_success = move_down(move_group_right_, 0.38, "右臂",
                                           right_fruit.offset_x, right_fruit.offset_y);
      
      if (right_move_success) {
        RCLCPP_INFO(logger_, "右侧水果抓取完成");
        
        // 发布右爪子数据0（关闭）
        auto right_msg = std_msgs::msg::Int32();
        right_msg.data = 0;
        zhuazi_right_publisher_->publish(right_msg);
        RCLCPP_INFO(logger_, "发布到 /zhuazi_right: 0 (关闭)");
        
        // 等待1.5秒让爪子动作完成
        RCLCPP_INFO(logger_, "等待1.5秒让爪子动作完成...");
        std::this_thread::sleep_for(std::chrono::milliseconds(1500));
        
        result_message += " - 右侧抓取成功";
        result_message += " - 右侧(类型=" + std::to_string(right_fruit.type) + 
                         ", offset=" + std::to_string(right_fruit.get_offset()) + ")";
        successful_grabs++;
        
        feedback->progress_percentage = 90.0;
        feedback->current_status = "右侧抓取完成";
        goal_handle->publish_feedback(feedback);
      } else {
        RCLCPP_WARN(logger_, "右侧机械臂运动失败");
        result_message += " - 右侧运动失败";
      }
      
    } else {
      RCLCPP_WARN(logger_, "右侧未找到匹配的水果");
      result_message += " - 右侧未找到匹配水果";
    }
  } else {
    RCLCPP_INFO(logger_, "右侧类型数组为空，跳过右侧抓取");
  }
  
  // 最终完成
  feedback->progress_percentage = 100.0;
  feedback->current_status = "抓取任务完成";
  goal_handle->publish_feedback(feedback);
  
  // 动作完成
  if (successful_grabs > 0) {
    result->success = true;
    result->message = result_message + " (成功执行" + std::to_string(successful_grabs) + "次抓取)";
    goal_handle->succeed(result);
    RCLCPP_INFO(logger_, "动作类型2执行成功，成功执行%d次抓取", successful_grabs);
  } else {
    result->success = false;
    result->message = "抓取任务失败 - 未成功执行任何抓取动作";
    goal_handle->abort(result);
    RCLCPP_WARN(logger_, "抓取任务失败 - 未成功执行任何抓取动作");
  }
}

} // namespace arm_control

